package com.rockwell.robot2015.model.states.running.sequence;

import java.security.SecureRandom;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class FindingTreasureSequence extends RobotRunningSequence {
	public static final int TURN_DEGREES = 15;
	public static final int SCAN_RANGE = 45;
	public static final int BIG_STEP = 60;
	public static final int SMALL_STEP = 20;
	
	static int m_turnedDegrees = 0;
	static int m_turnDirection = 1;
	static boolean m_turnedBack = false;
	
	static SecureRandom m_random = new SecureRandom();
	
	public enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start finding treasure" , 2);

				m_turnedDegrees = 0;
				m_turnedBack = false;
				
				Robot robot = machine.robot();
				IDetector detector = machine.robot().getTreasureDetector();
				
				if (detector.isPresent()) {
					machine.changeState(TreasureFound);
				}
				else {
					robot.sendCommand(RobotAction.MoveWithSensor, BIG_STEP);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				
					m_turnedBack = false;
					machine.changeState(Detect);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Detect {

			@Override
			public void enter(IStateMachine machine) {
				// do nothing
				
				//machine.robot().log("Enter FindingTreaure.Detect" , 2);
			}

			@Override
			public void move(IStateMachine machine) {
				//machine.robot().log("Move FindingTreaure.Detect" , 2);

				Robot robot = machine.robot();
				
				IDetector detector = machine.robot().getTreasureDetector();
				
				if (detector.isPresent()) {
					machine.changeState(TreasureFound);
				}
				else if (m_sequence.turnedDegrees() < SCAN_RANGE) {
					machine.changeState(Scan);
				}
				else {
					if (m_turnedBack) {
						machine.changeState(Move);
					}
					else {
						robot.sendCommand(RobotAction.TurnDegree, 
								-(SCAN_RANGE + m_turnedDegrees) * m_turnDirection);
						
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						m_turnedBack = true;
						m_turnedDegrees = 0;
						
						if (machine.getState() != Scan) {
							machine.changeState(Scan);
						}
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// do nothing

				//machine.robot().log("Exit FindingTreaure.Detect" , 2);
			}
			
		},
		Scan {

			@Override
			public void enter(IStateMachine machine) {
				// do nothing
				
				//machine.robot().log("Enter FindingTreaure.Turn" , 2);
			}

			@Override
			public void move(IStateMachine machine) {
				//machine.robot().log("Move FindingTreaure.Turn" , 2);

				// Send Turn command
				Robot robot = machine.robot();
				IRobotSensors sensor = robot.getSensor();
				
				if (!sensor.isLeftFrontTooClose() && !sensor.isLeftBackTooClose()) {
					robot.log(String.format("Turn %d degrees", TURN_DEGREES) , 4);
					
					robot.sendCommand(RobotAction.TurnDegree, TURN_DEGREES);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
				else {
					robot.log("Too close to left wall", 4);
				}
				
				m_turnedDegrees += TURN_DEGREES;

				machine.changeState(Detect);
			}

			@Override
			public void exit(IStateMachine machine) {
				// do nothing
				
				//machine.robot().log("Exit FindingTreaure.Turn" , 2);
			}
			
		},
		Move {
			@Override
			public void enter(IStateMachine machine) {
				m_turnedDegrees = 0;
				m_turnedBack = false;
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				robot.sendCommand(RobotAction.MoveWithSensor, BIG_STEP);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
				
				IRobotSensors sensor = robot.getSensor();
				
				if (sensor.detectFrontWall()) {
					robot.turnBack();
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					robot.sendCommand(RobotAction.MoveWithSensor, SMALL_STEP);
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					
					if (m_random.nextBoolean()) {
						if (!sensor.detectLeftWall()) {
							robot.sendCommand(RobotAction.TurnLeft);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
						else if (!sensor.detectRightWall()) {
							robot.sendCommand(RobotAction.TurnRight);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
					}
					else {
						if (!sensor.detectRightWall()) {
							robot.sendCommand(RobotAction.TurnRight);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
						else if (!sensor.detectLeftWall()) {
							robot.sendCommand(RobotAction.TurnLeft);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
					}
				}
				
				machine.changeState(Detect);
			}

			@Override
			public void exit(IStateMachine machine) {
				// do nothing
				//machine.robot().log("Exit FindingTreaure.Move" , 2);
			}
			
		},
		TreasureFound {

			@Override
			public void enter(IStateMachine machine) {
				// do nothing
				machine.robot().log("Treasure Found" , 2);
			}

			@Override
			public void move(IStateMachine machine) {
				// do nothing
			}

			@Override
			public void exit(IStateMachine machine) {
				// do nothing
				//machine.robot().log("Exit FindingTreaure.TreasureFound" , 2);
			}
			
		};
		
		static FindingTreasureSequence m_sequence;
		
		public static void setSequence(FindingTreasureSequence sequence) {
			m_sequence = sequence;
		}
	}
	
	public FindingTreasureSequence(RobotRunningStateMachine running) {
		super(running, State.Start);
		
		State.setSequence(this);
		
		m_currState = State.Start;
	}

	@Override
	public void start() {
		Robot robot = m_running.robot();
		
		//robot.log("Finding treasure", 1);
		
		m_turnedDegrees = 0;
		m_turnedBack = false;

		m_currState = State.Start;
		m_nextState = null;
		
		robot.enableTreasureDetector(true);
		//robot.enableObstacleDetector(true);
		robot.enableMaze1Detector(true);
		robot.enableMaze2Detector(true);
		robot.enableMaze3Detector(true);
		robot.enableMazeEntranceGuideDetector(false);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(true);
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.TreasureFound);
	}

	@Override
	public boolean failed() {
		return false;
	}
	
	@Override
	public void reset() {
		super.reset();
		resetTurnedDegrees();
	}
	
	public int turnedDegrees() {
		return m_turnedDegrees;
	}
	
	public void onTurned(int degrees) {
		m_turnedDegrees += degrees;
	}
	
	public void resetTurnedDegrees() {
		m_turnedDegrees = 0;
	}
	
	public void reverse() {
		m_turnedDegrees = 0;
		m_turnDirection = -m_turnDirection;
	}
	
	public int getDirection() {
		return m_turnDirection;
	}
}
